/*
 * Astar.cpp
 *
 *  Created on: 02.04.2012
 *      Author: ignacio
 */

#include "Astar.h"

#include <string>
using std::string;

namespace neurignacio
{

template <> VectorVertex::timestamp_t AStar_Heuristic(VectorVertex* u, VectorVertex* v)
// For Vector3, I will use euclidian distance as heuristic
{
	Vector3 d = *(v->value) - *(u->value);
	return d.magnitude();
}

VectorPath VertexPathToVectorPath(Path<VectorVertex>& path)
{
	const size_t N=path.size();
	VectorPath vectorPath;
	vectorPath.resize(N);
	for (register size_t i=0; i<N; ++i)
		vectorPath[i]=*(path[i].value);

	return vectorPath;
}

void AStar_Example(void)
{
//	cout << "Creating Vector Points: " << endl;
	Vector3 aVector(1,4);
	Vector3 bVector(2,4);
	Vector3 cVector(3,4);
	Vector3 dVector(4,4);
	Vector3 eVector(1,3);
	Vector3 fVector(2,3);
	Vector3 gVector(3,3);
	Vector3 hVector(4,3);
	Vector3 iVector(1,2);
	Vector3 jVector(2,2);
	Vector3 kVector(3,2);
	Vector3 lVector(4,2);
	Vector3 mVector(1,1);
	Vector3 nVector(2,1);
	Vector3 oVector(3,1);
	Vector3 pVector(4,1);

	VectorVertex a(aVector);
	VectorVertex b(bVector);
	VectorVertex c(cVector);
	VectorVertex d(dVector);
	VectorVertex e(eVector);
	VectorVertex f(fVector);
	VectorVertex g(gVector);
	VectorVertex h(hVector);
	VectorVertex i(iVector);
	VectorVertex j(jVector);
	VectorVertex k(kVector);
	VectorVertex l(lVector);
	VectorVertex m(mVector);
	VectorVertex n(nVector);
	VectorVertex o(oVector);
	VectorVertex p(pVector);

	VectorAdjacencyList aList;
	VectorAdjacencyList bList;
	VectorAdjacencyList cList;
	VectorAdjacencyList dList;
	VectorAdjacencyList eList;
	VectorAdjacencyList fList;
	VectorAdjacencyList gList;
	VectorAdjacencyList hList;
	VectorAdjacencyList iList;
	VectorAdjacencyList jList;
	VectorAdjacencyList kList;
	VectorAdjacencyList lList;
	VectorAdjacencyList mList;
	VectorAdjacencyList nList;
	VectorAdjacencyList oList;
	VectorAdjacencyList pList;

	aList.push_back(a,b);
	aList.push_back(a,e);

	bList.push_back(b,a);
	bList.push_back(b,c);
	bList.push_back(b,f);

	cList.push_back(c,b);
	cList.push_back(c,d);
	cList.push_back(c,g);

	dList.push_back(d,c);
	dList.push_back(d,h);

	eList.push_back(e,a);
	eList.push_back(e,f,1);
	eList.push_back(e,i,1);

	fList.push_back(f,e,1);
	fList.push_back(f,b);
	fList.push_back(f,g,10);
	fList.push_back(f,j,10);

	gList.push_back(g,f,10);
	gList.push_back(g,c);
	gList.push_back(g,h);
	gList.push_back(g,k,10);

	hList.push_back(h,d);
	hList.push_back(h,g,10);
	hList.push_back(h,l,10);

	iList.push_back(i,e,1);
	iList.push_back(i,j,1);
	iList.push_back(i,m);

	jList.push_back(j,i,1);
	jList.push_back(j,f,10);
	jList.push_back(j,k,10);
	jList.push_back(j,n);

	kList.push_back(k,j,1);
	kList.push_back(k,g,10);
	kList.push_back(k,l,10);
	kList.push_back(k,o);

	lList.push_back(l,k,10);
	lList.push_back(l,h,10);
	lList.push_back(l,p);

	mList.push_back(m,i);
	mList.push_back(m,n);

	nList.push_back(n,m);
	nList.push_back(n,j);
	nList.push_back(n,o);

	oList.push_back(o,n);
	oList.push_back(o,k);
	oList.push_back(o,p);

	pList.push_back(p,o);
	pList.push_back(p,l);

	GraphVector G;
	G[&a] = &aList;
	G[&b] = &bList;
	G[&c] = &cList;
	G[&d] = &dList;
	G[&e] = &eList;
	G[&f] = &fList;
	G[&g] = &gList;
	G[&h] = &hList;
	G[&i] = &iList;
	G[&j] = &jList;
	G[&k] = &kList;
	G[&l] = &lList;
	G[&m] = &mList;
	G[&n] = &nList;
	G[&o] = &oList;
	G[&p] = &pList;


	cout << "Before:" << endl;
	cout << G;



	cout << "Dijkstra:"<< endl;
	G.Dijkstra(&a);
	G.PrintPath(&a,&p);

	cout << "A_Star:"<< endl;
	G.A_Star(&a,&p);
	G.PrintPath(&a,&p);
}

void Hyper_AStar_Example(void)
{
	Hypergraph<Vector3> H;

	Vector3 a(1,4);
	Vector3 b(2,4);
	Vector3 c(3,4);
	Vector3 d(4,4);
	Vector3 e(1,3);
	Vector3 f(2,3);
	Vector3 g(3,3);
	Vector3 h(4,3);
	Vector3 i(1,2);
	Vector3 j(2,2);
	Vector3 k(3,2);
	Vector3 l(4,2);
	Vector3 m(1,1);
	Vector3 n(2,1);
	Vector3 o(3,1);
	Vector3 p(4,1);

	H.addEdge(a,b);
	H.addEdge(a,e);

	H.addEdge(b,a);
	H.addEdge(b,c);
	H.addEdge(b,f);

	H.addEdge(c,b);
	H.addEdge(c,d);
	H.addEdge(c,g);

	H.addEdge(d,c);
	H.addEdge(d,h);

	H.addEdge(e,a);
	H.addEdge(e,f,1);
	H.addEdge(e,i,1);

	H.addEdge(f,e,1);
	H.addEdge(f,b);
	H.addEdge(f,g,10);
	H.addEdge(f,j,10);

	H.addEdge(g,f,10);
	H.addEdge(g,c);
	H.addEdge(g,h);
	H.addEdge(g,k,10);

	H.addEdge(h,d);
	H.addEdge(h,g,10);
	H.addEdge(h,l,10);

	H.addEdge(i,e,1);
	H.addEdge(i,j,1);
	H.addEdge(i,m);

	H.addEdge(j,i,1);
	H.addEdge(j,f,10);
	H.addEdge(j,k,10);
	H.addEdge(j,n);

	H.addEdge(k,j,1);
	H.addEdge(k,g,10);
	H.addEdge(k,l,10);
	H.addEdge(k,o);

	H.addEdge(l,k,10);
	H.addEdge(l,h,10);
	H.addEdge(l,p);

	H.addEdge(m,i);
	H.addEdge(m,n);

	H.addEdge(n,m);
	H.addEdge(n,j);
	H.addEdge(n,o);

	H.addEdge(o,n);
	H.addEdge(o,k);
	H.addEdge(o,p);

	H.addEdge(p,o);
	H.addEdge(p,l);

	cout << "Dijkstra:"<< endl;
	H.Dijkstra(a);
	H.PrintPath(a,p);

	cout << "A_Star:"<< endl;
	H.A_Star(a,p);
	H.PrintPath(a,p);
}

void Hypergraph_Example()
{
	Hypergraph<Vector3> H;
	Vector3 u(1,2,3);
	Vector3 v(4,5,6);
	Vector3 x(7,8,9);
	Vector3 y(10,11,12);
	//H.addEdge(u,v,5.0f);
	real w = 5;
	H.addEdge(u,v,w);
//	cout << "House size: " << H.house.size() << " u=" << H.house.front() << ", v=" << H.house.back() << endl;
//	cout << "VertexHouse size: " << H.vertexHouse.size() << " U=" << H.vertexHouse.front() << ", V=" << H.vertexHouse.back() <<endl;
//	cout << "House size: " << H.house.size() << " u=" << H.house.front() << ", v=" << H.house.back() << endl;
//	cout << "VertexHouse size: " << H.vertexHouse.size() << " U=" << H.vertexHouse.front() << ", V=" << H.vertexHouse.back() <<endl;
	H.addEdge(v,u,w);
//	cout << "Graph H (size=" << H.size() << ")" << endl;
//	cout << "H.begin()->first: " << *(H.begin()->first) << endl;
//	cout << "Adjacency List for H.begin(): " << *(H.begin()->second) << endl;
//	H.house[0]=Vector3(7,8,9);
	H.addEdge(u,x);
	H.addEdge(x,v);
	H.addEdge(y,u);
//	cout << H << endl;
	Hypergraph<Vector3>::Vertex_t* p = H.getPointer(x);
	cout << *p << endl;

}

template <> Hypergraph<Vector3>::Vertex_t* Hypergraph<Vector3>::getPointer(const Vector3& u, real distance) const
{
	real d = distance*distance; // absolute value
	Vertex_t* pointer=0;
	Hypergraph_const_iterator it=this->begin();
	while (!pointer && it!=this->end())
	{
		Vector3 diff = *(it->first->value) - u;
		if (diff.squareMagnitude() <= d)
			pointer = it->first;
		++it;
	}
	return pointer;
}

} // end namespace neurignacio

